#include <Arduino.h>
#include "wifi_control.h"
#include "ota_manager.h"

// 电机引脚配置
const int AIN1 = 18, AIN2 = 19, BIN1 = 4, BIN2 = 5, nSLEEP = 34;

// 全局对象
WiFiControl wifiControl;
OTAManager otaManager;

// 电机安全控制（带状态跟踪）
void motorControl(const String& cmd) {
    static String lastCmd = "";
    if (cmd == lastCmd) return;  // 避免重复执行相同命令
    
    Serial.println("Motor executing: " + cmd);
    
    // 先停止所有电机
    digitalWrite(AIN1, LOW);
    digitalWrite(AIN2, LOW);
    digitalWrite(BIN1, LOW);
    digitalWrite(BIN2, LOW);
    delay(20);  // 确保电机完全停止

    if (cmd == "forward") {
        digitalWrite(AIN1, HIGH);
        digitalWrite(BIN1, HIGH);
    } 
    else if (cmd == "backward") {
        digitalWrite(AIN2, HIGH);
        digitalWrite(BIN2, HIGH);
    }
    else if (cmd == "left") {
        digitalWrite(AIN2, HIGH);  // 左轮后退
        digitalWrite(BIN1, HIGH);  // 右轮前进
    }
    else if (cmd == "right") {
        digitalWrite(AIN1, HIGH);  // 左轮前进
        digitalWrite(BIN2, HIGH);  // 右轮后退
    }
    // "stop"命令已经通过前面的LOW处理

    lastCmd = cmd;
}

void setup() {
    Serial.begin(115200);
    
    // 初始化电机引脚
    pinMode(AIN1, OUTPUT);
    pinMode(AIN2, OUTPUT);
    pinMode(BIN1, OUTPUT);
    pinMode(BIN2, OUTPUT);
    pinMode(nSLEEP, OUTPUT);
    digitalWrite(nSLEEP, HIGH);  // 启用电机驱动

    // 启动WiFi控制
    wifiControl.begin(motorControl);
    
    // 启动OTA（假设WiFi已连接）
    otaManager.begin("30243024"); // 使用你的OTA密码
    
    Serial.println("System Ready");
}

void loop() {
    wifiControl.update();
    otaManager.handle(); // 保持OTA功能
    
    // 后续可添加传感器代码...
    // 例如：if (ultrasonicDistance < 10) motorControl("stop");
}